#ifndef SERVO_H
#define SERVO_H

#include <stdio.h>
#include <wiringPi.h>
#include <softPwm.h>

class Servo{
	int angle;
	char PIN_SERVO;
	bool left=false;
	bool right=false;
	
	public:
	Servo(char pin) :angle(15),PIN_SERVO(pin) {}
	
	void init(void)
	{
		softPwmCreate(PIN_SERVO, angle, 100);
	}

	void reset(void)
	{
		softPwmWrite(PIN_SERVO,15);
		return;
	}
	
	inline void run(void)
	{
		softPwmWrite(PIN_SERVO,angle);
	}
	
	void turn_left(void)
	{
		if(right) right=false;
		
		if(!left) angle -= 1;
		if(angle<0) 
		{
			angle = 0;
			left = true;
		}
		run();
	}
	void turn_right(void)
	{
		if(left) left=false;
		
		if(!right) angle += 1;
		if(angle>20) 
		{
			angle = 20;
			right = true;
		}
		run();
	}

	void stop(void)
	{
		softPwmStop(PIN_SERVO);
		return;
	}
	
	bool hit_left(void)
	{
		return left;
	}
	bool hit_right(void)
	{
		return right;
	}
	
	//-------------------------application
	void scan(int times, int delay_time)
	{
		while(times--){
			while(!hit_left())
			{
				turn_left();
				delay(delay_time);
			}
			while(!hit_right())
			{
				turn_right();
				delay(delay_time);
			}
		}
	}
	
};

#endif
